#include "geometry_msgs/msg/twist.hpp"
#include "ncurses.h"
#include "rclcpp/rclcpp.hpp"

class kbnode : public rclcpp::Node
{
  private:
	rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub;
	rclcpp::TimerBase::SharedPtr scan_timer;
	void scan_thread_func()
	{
		static char input;
		static int count = 0;
		geometry_msgs::msg::Twist twist;

		char temp = getch();
		if (temp != ERR)
		{
			input = temp;
			count = 0;
		}
		else
		{
			// keep input for a short period of time
			if (count > 20)
				input = ERR;
			else
				count++;
		}

		switch (input)
		{
			case 'w':
				twist.linear.x = 1;
				break;
			case 'a':
				twist.angular.z = -0.5;
				break;
			case 's':
				twist.linear.x = -1;
				break;
			case 'd':
				twist.angular.z = 0.5;
				break;
			case 'W':
				twist.linear.x = 2;
				break;
			case 'S':
				twist.linear.x = -2;
				break;
			default:  // all other keys for brake
				// -1 for error
				twist.angular.x = 0;
				twist.angular.y = 0;
				twist.angular.z = 0;
				twist.linear.x = 0;
				twist.linear.y = 0;
				twist.linear.z = 0;
				break;
		}

		move(0, 0);
		printw("linear: %lf, angular: %lf\n", twist.linear.x, twist.angular.z);
		refresh();
		this->twist_pub->publish(twist);
	}

  public:
	kbnode(std::string name) : Node(name)
	{
		this->twist_pub = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
		initscr();
		timeout(30);		  // non blocking read
		keypad(stdscr, TRUE); // enable special keys
		noecho();			  // disable input echo
		this->scan_timer = this->create_wall_timer(std::chrono::milliseconds(30),
												   std::bind(&kbnode::scan_thread_func, this));
	}
	~kbnode()
	{
		endwin();
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<kbnode>("kbinput");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
